#pragma once
#ifndef COLLLISION_DETECTOR_H
#define COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include <CGAL/squared_distance_2.h>
#include <CGAL/minkowski_sum_2.h>

class CollisionDetector
{
public:
	
	////////////////////////
	// CTORS
	////////////////////////
	/*	Parameters:
			radius	- radius of robot
			Room	- describes the boundary of the scenario
	*/
	CollisionDetector(vector<Polygon_2>	robot_polygons, Obstacles *obs);
	
	~CollisionDetector(){};

	////////////////////////
	// Queries
	////////////////////////
	
	Polygon_2 negative_polygon(Polygon_2 polygon);

	/*	Check if the given configuration is collision free (returns true for free samples) */
	bool single_robot_valid_conf(int index, Conf robot_conf);
	bool valid_conf(Conf robot_1_conf, Conf robot_2_conf);

	/*	Validate the connection between two configurations by 
		sampling along a line.
		Input:	* Start and target configurations
				* Eps value that determines the size of the steps 
					that are taken along the line
	*/

	bool	single_robot_local_planner(int index, Point_2 start, Point_2 target, double step_size);
	bool	local_planner(ConfSet start, ConfSet target, double step_size);

	Polygon_2 get_new_polygon( Conf new_robot_point, Polygon_2 original_robot );


	////////////////////////
	// Data Members
	////////////////////////
	vector<Polygon_2>			m_robot_polygons;
	Obstacles					*m_obs;		// Collection of polygonal obstacles
	Polygon_set_2				m_robot_1_poly_set; // Polygon set, for collision detection
	Polygon_set_2				m_robot_2_poly_set; // Polygon set, for collision detection
	Polygon_set_2				m_robots_poly_set; // Polygon set, for collision detection of both robots
};

#endif